/*
 * OpticalFlow.c
 *
 *  Created on: May 6, 2021
 *      Author: 90797
 */
#include "OpticalFlow.h"
#include "main.h"
#include "math.h"

extern UART_HandleTypeDef huart2;

#define OpticalFlow_USART huart2

#define OpticalFlow_BUF_SIZE 14
uint8_t OpticalFlow_RX_BUF[OpticalFlow_BUF_SIZE];

const static uint8_t Sensor_cfg[]={
  0x12, 0x80,   0x11, 0x30,   0x1b, 0x06,   0x6b, 0x43,   0x12, 0x20,   0x3a, 0x00,   0x15, 0x02,   0x62, 0x81,   0x08, 0xa0,   0x06, 0x68,   0x2b, 0x20,
  0x92, 0x25,   0x27, 0x97,   0x17, 0x01,   0x18, 0x79,   0x19, 0x00,   0x1a, 0xa0,   0x03, 0x00,   0x13, 0x00,   0x01, 0x13,   0x02, 0x20,   0x87, 0x16,
  0x8c, 0x01,   0x8d, 0xcc,   0x13, 0x07,   0x33, 0x10,   0x34, 0x1d,   0x35, 0x46,   0x36, 0x40,   0x37, 0xa4,   0x38, 0x7c,   0x65, 0x46,   0x66, 0x46,
  0x6e, 0x20,   0x9b, 0xa4,   0x9c, 0x7c,   0xbc, 0x0c,   0xbd, 0xa4,   0xbe, 0x7c,   0x20, 0x09,   0x09, 0x03,   0x72, 0x2f,   0x73, 0x2f,   0x74, 0xa7,
  0x75, 0x12,   0x79, 0x8d,   0x7a, 0x00,   0x7e, 0xfa,   0x70, 0x0f,   0x7c, 0x84,   0x7d, 0xba,   0x5b, 0xc2,   0x76, 0x90,   0x7b, 0x55,   0x71, 0x46,
  0x77, 0xdd,   0x13, 0x0f,   0x8a, 0x10,   0x8b, 0x20,   0x8e, 0x21,   0x8f, 0x40,   0x94, 0x41,   0x95, 0x7e,   0x96, 0x7f,   0x97, 0xf3,   0x13, 0x07,
  0x24, 0x58,   0x97, 0x48,   0x25, 0x08,   0x94, 0xb5,   0x95, 0xc0,   0x80, 0xf4,   0x81, 0xe0,   0x82, 0x1b,   0x83, 0x37,   0x84, 0x39,   0x85, 0x58,
  0x86, 0xff,   0x89, 0x15,   0x8a, 0xb8,   0x8b, 0x99,   0x39, 0x98,   0x3f, 0x98,   0x90, 0xa0,   0x91, 0xe0,   0x40, 0x20,   0x41, 0x28,   0x42, 0x26,
  0x43, 0x25,   0x44, 0x1f,   0x45, 0x1a,   0x46, 0x16,   0x47, 0x12,   0x48, 0x0f,   0x49, 0x0d,   0x4b, 0x0b,   0x4c, 0x0a,   0x4e, 0x08,   0x4f, 0x06,
  0x50, 0x06,   0x5a, 0x56,   0x51, 0x1b,   0x52, 0x04,   0x53, 0x4a,   0x54, 0x26,   0x57, 0x75,   0x58, 0x2b,   0x5a, 0xd6,   0x51, 0x28,   0x52, 0x1e,
  0x53, 0x9e,   0x54, 0x70,   0x57, 0x50,   0x58, 0x07,   0x5c, 0x28,   0xb0, 0xe0,   0xb1, 0xc0,   0xb2, 0xb0,   0xb3, 0x4f,   0xb4, 0x63,   0xb4, 0xe3,
  0xb1, 0xf0,   0xb2, 0xa0,   0x55, 0x00,   0x56, 0x40,   0x96, 0x50,   0x9a, 0x30,   0x6a, 0x81,   0x23, 0x33,   0xa0, 0xd0,   0xa1, 0x31,   0xa6, 0x04,
  0xa2, 0x0f,   0xa3, 0x2b,   0xa4, 0x0f,   0xa5, 0x2b,   0xa7, 0x9a,   0xa8, 0x1c,   0xa9, 0x11,   0xaa, 0x16,   0xab, 0x16,   0xac, 0x3c,   0xad, 0xf0,
  0xae, 0x57,   0xc6, 0xaa,   0xd2, 0x78,   0xd0, 0xb4,   0xd1, 0x00,   0xc8, 0x10,   0xc9, 0x12,   0xd3, 0x09,   0xd4, 0x2a,   0xee, 0x4c,   0x7e, 0xfa,
  0x74, 0xa7,   0x78, 0x4e,   0x60, 0xe7,   0x61, 0xc8,   0x6d, 0x70,   0x1e, 0x39,   0x98, 0x1a
};

int16_t flow_x_integral,flow_y_integral,integration_timespan;

static void OpticalFlow_UART_Port(uint8_t Data);

static void OpticalFlow_UART_Port(uint8_t Data)
{
	HAL_UART_Transmit(&OpticalFlow_USART, &Data, 1, 1);
}

void OpticalFlow_Init(void)
{
	OpticalFlow_UART_Port(0xAA); //¿ªÆôÅäÖÃÖ¸Áî

	OpticalFlow_UART_Port(0xAB);  //Ä£¿éÄÚ²¿²ÎÊýÅäÖÃÖ¸Áî
	OpticalFlow_UART_Port(0x96);
	OpticalFlow_UART_Port(0x26);
	OpticalFlow_UART_Port(0xbc);
	OpticalFlow_UART_Port(0x50);
	OpticalFlow_UART_Port(0x5c);

	uint16_t len,i;
	len = sizeof(Sensor_cfg);
	for(i=0;i<len;i+=2)//´«¸ÐÆ÷²ÎÊýÅäÖÃÖ¸Áî
	{
		OpticalFlow_UART_Port(0xBB);//´«¸ÐÆ÷²ÎÊýÅäÖÃÖ¸Áî
		OpticalFlow_UART_Port(0xdc);
		OpticalFlow_UART_Port(Sensor_cfg[i]);
		OpticalFlow_UART_Port(Sensor_cfg[i+1]);
		OpticalFlow_UART_Port(0xdc^Sensor_cfg[i]^Sensor_cfg[i+1]);
	}

	OpticalFlow_UART_Port(0xDD);//¹Ø±ÕÅäÖÃÖ¸Áî

	uint8_t flag=0;
	while(flag!=0x55){HAL_UART_Receive(&OpticalFlow_USART,&flag,1, 1);};
	HAL_UART_Receive_DMA(&OpticalFlow_USART, OpticalFlow_RX_BUF, OpticalFlow_BUF_SIZE);
//	HAL_UART_Receive_IT(&OpticalFlow_USART, OpticalFlow_RX_BUF, OpticalFlow_BUF_SIZE);
}



void OpticalFlow_Read(float *data)		//50Hz
{
	data[0]=flow_x_integral;
	data[1]=flow_y_integral;
}

void OpticalFlow_DMA(void)
{
	if(OpticalFlow_RX_BUF[0]==0xFE&&OpticalFlow_RX_BUF[1]==0x0A)
	{
		if(OpticalFlow_RX_BUF[10]==0xF5)
		{
			uint8_t XOR=0;
			for(uint8_t i=0 ;i<10;i++)
			{
				XOR=XOR^OpticalFlow_RX_BUF[i+2];
			}
			if(XOR==OpticalFlow_RX_BUF[12])
			{
				flow_x_integral=(OpticalFlow_RX_BUF[3]<<8)+OpticalFlow_RX_BUF[2];
				flow_y_integral=(OpticalFlow_RX_BUF[5]<<8)+OpticalFlow_RX_BUF[4];
				integration_timespan=(OpticalFlow_RX_BUF[7]<<8)+OpticalFlow_RX_BUF[6];
			}
		}
	}
}
